/*****************************************************************************

Copyright (c) 2004 SensAble Technologies, Inc. All rights reserved.

OpenHaptics(TM) toolkit. The material embodied in this software and use of
this software is subject to the terms and conditions of the clickthrough
Development License Agreement.

For questions, comments or bug reports, go to forums at: 
    http://dsc.sensable.com

Module Name:

  HelloHapticDevice.c

Description: 

  This application creates a gravity well, which will attract
  the device towards its center when the device enters its proximity.  

*******************************************************************************/
#ifdef  _WIN64
#pragma warning (disable:4996)
#endif

#include <stdio.h>
#include <assert.h>

#if defined(WIN32)
# include <windows.h>
# include <conio.h>
#else
# include "conio.h"
# include <string.h>
#endif

#include <HD/hd.h>
#include <HDU/hduError.h>
#include <HDU/hduVector.h>

#include <Eigen/Core>


#include <boost/thread/condition.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
//#include <boost/thread/xtime.hpp>

#include <no_control.h>
#include <gravity_well_control.h>

using namespace std;

#define CONTROL_CYCLE 0.001	/* s */

#define NUMBER_OF_PHANToM_Omni 1
#define NAME_OF_PHANToM_Omni_0 "LeftOne"
#define NAME_OF_PHANToM_Omni_1 "RightOne"

bool bRun;
boost::condition sync_cond;
boost::mutex sync_mutex;
vector<HHD> hHD;

/* Holds data retrieved from HDAPI. */
typedef struct 
{
	HDboolean m_buttonState;       /* Has the device button has been pressed. */
	hduVector3Dd m_devicePosition; /* Current device coordinates. */
	HDErrorInfo m_error;
} DeviceData;

static DeviceData gServoDeviceData;

controllers::No_Control NoControl;
controllers::Gravity_Well_Control GravityWellControl;

HDCallbackCode HDCALLBACK dummyCallback(void *data);

void Controller(void)
{
	int nButtons = 0;
	bool curButtonState = false;
	static bool preButtonState = false;
	static int curMode = -1;
	static int nxtMode = 0;

	//static boost::unique_lock<boost::mutex> lock(sync_mutex);
	//sync_cond.wait(lock);

	for (int i = 0; i < hHD.size(); i++)
	{
		hdMakeCurrentDevice(hHD[i]);

		/* Retrieve the current button(s). */
		hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);

		/* In order to get the specific button 1 state, we use a bitmask to
		test for the HD_DEVICE_BUTTON_1 bit. */
		if (nButtons & HD_DEVICE_BUTTON_1)
		{
			curButtonState = true;
		}
	}

	if (curButtonState && !preButtonState)
	{
		if (curMode == 0)
		{
			nxtMode = 1;
		}
		if (curMode ==1)
		{
			nxtMode = 0;
		}
	}

	preButtonState = curButtonState;

	if (curMode != nxtMode)
	{
		switch (nxtMode)
		{
		case 0:
			NoControl.starting(boost::posix_time::microsec_clock::local_time());
			break;
		case 1:
			GravityWellControl.starting(boost::posix_time::microsec_clock::local_time());
			break;
		}
	}

	curMode = nxtMode;

	switch(curMode)
	{
	case 0:
		NoControl.update(boost::posix_time::microsec_clock::local_time(),boost::posix_time::seconds(CONTROL_CYCLE));
		break;
	case 1:
		GravityWellControl.update(boost::posix_time::microsec_clock::local_time(),boost::posix_time::seconds(CONTROL_CYCLE));
		break;
	default:
		break;
	}

	//boost::this_thread::sleep(boost::posix_time::seconds(CONTROL_CYCLE));

}

/*******************************************************************************
 Main function.
 Initializes the device, starts the schedule, creates a schedule callback
 to handle gravity well forces, waits for the user to press a button, exits
 the application.
*******************************************************************************/
int main(int argc, char* argv[])
{    
    int i;
	HDErrorInfo error;
    HDSchedulerHandle hDummyCallback;

	hHD.clear();

    /* Initialize the device, must be done before attempting to call any hd 
       functions. Passing in HD_DEFAULT_DEVICE causes the default device to be 
       initialized. */
    hHD.push_back(hdInitDevice(HD_DEFAULT_DEVICE));
    if (HD_DEVICE_ERROR(error = hdGetError())) 
    {
        hduPrintError(stderr, &error, "Failed to initialize haptic device(the left one)");
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;
    }

	printf("Hello Haptic Device!\n");
	printf("Found device model: %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));

	NoControl.init(hHD);
	GravityWellControl.init(hHD);

    /* Schedule the main callback that will render forces to the device. */
    hDummyCallback = hdScheduleAsynchronous(
        dummyCallback, 0, 
        HD_MAX_SCHEDULER_PRIORITY);
	for (i = 0; i < hHD.size(); i++)
	{
		hdMakeCurrentDevice(hHD[i]);
		hdEnable(HD_FORCE_OUTPUT);
		hdDisable(HD_ONE_FRAME_LIMIT);
	}
    hdStartScheduler();

	bRun = true;

	//SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);

	//boost::thread thread_Controller(Controller);
	//SetThreadPriority(thread_Controller.native_handle(), THREAD_PRIORITY_TIME_CRITICAL);

    /* Check for errors and abort if so. */
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to start scheduler");
        fprintf(stderr, "\nPress any key to quit.\n");
        return -1;
    }	

    /* Wait until the user presses a key.  Meanwhile, the scheduler
       runs and applies forces to the device. */
    printf("Feel around for the gravity well...\n");
    printf("Press any key to quit.\n\n");
    //while (!_kbhit())
    //{
    //    /* Periodically check if the gravity well callback has exited. */
    //    if (!hdWaitForCompletion(hDummyCallback, HD_WAIT_CHECK_STATUS))
    //    {
    //        fprintf(stderr, "Press any key to quit.\n");     
    //        getch();
    //        break;
    //    }
    //}
	getch();

    /* For cleanup, unschedule callback and stop the scheduler. */
	bRun = false;
	//thread_Controller.join();
	hdStopScheduler();
	hdUnschedule(hDummyCallback);

    /* Disable the device. */
	for (i = 0; i < hHD.size(); i++)
	{
		hdDisableDevice(hHD[i]);
	}

    return 0;
}

/*****************************************************************************/

HDCallbackCode HDCALLBACK dummyCallback(void *data)
{
	//HDErrorInfo error;
	
	//for (int i = 0; i < 2; i++)
	//{
	//	hdMakeCurrentDevice(hHD[i]);

	//	/* Check for errors and abort the callback if a scheduler error
	//	is detected. */
	//	if (HD_DEVICE_ERROR(error = hdGetError()))
	//	{
	//		//hduPrintError(stderr, &error, 
	//		//	"Error detected while rendering gravity well\n");

	//		if (hduIsSchedulerError(&error))
	//		{
	//			return HD_CALLBACK_DONE;
	//		}
	//	}
	//}
	for (int i = 0; i < hHD.size(); i++)
	{
		hdMakeCurrentDevice(hHD[i]);

		/* Begin haptics frame.  ( In general, all state-related haptics calls
		should be made within a frame. ) */
		hdBeginFrame(hHD[i]);
	}

	Controller();
	
	for (int i = 0; i < hHD.size(); i++)
	{
		hdMakeCurrentDevice(hHD[i]);

		/* End haptics frame. */
		hdEndFrame(hHD[i]);
	}

	/* Signify that the callback should continue running, i.e. that
       it will be called again the next scheduler tick. */
    return HD_CALLBACK_CONTINUE;
}